#include <pio.h>
#include <system.h>
#include <std.h>
#include <error.h>
#include <irq.h>
#include <asm.h>
#include <8259.h>

#define CMOS 0x70
#define CMOS_RD 0x71
#define CMOS_GET_SEC 0x0
#define PIC1 0x20
#define PIC2 0xa0
#define PIC_INIT 0x11
#define PIC1_REG 0x21
#define PIC2_REG 0xa1
#define DELAY_PRT 0x80

#define PIC0_CMD 0x20
#define PIC1_CMD 0xa0
#define PIC0_DAT 0x21
#define PIC1_DAT 0xa1


#define IRQMAP1 0x20
#define IRQMAP2 0x28

#define RMIRQ1 0x8
#define RMIRQ2 0x70

/* IRQ map:
PIC1:
tim		kbd		pic		sp2		sp1		pp2		dsk		pp1
0		0		1		1		1		1		1		1
0xFC
PIC2:
rtc		cga		RES		RES		RES		fpu		hdc		RES
1		1		1		1		1		1		0		1
0xBF
*/

#define MASKALL 0xff
#define ALLOWALL 0


static inline void write_cmd0(unsigned char val)
{
	outb(PIC0_CMD,val);
}

static inline void write_cmd1(unsigned char val)
{
	outb(PIC1_CMD,val);
}

static inline void write_dat0(unsigned char val)
{
	outb(PIC0_DAT,val);
}

static inline void write_dat1(unsigned char val)
{
	outb(PIC1_DAT,val);
}

static inline unsigned char read_cmd0()
{
	return inb(PIC0_CMD);
}

static inline unsigned char read_cmd1()
{
	return inb(PIC1_CMD);
}

static inline unsigned char read_dat0()
{
	return inb(PIC0_DAT);
}

static inline unsigned char read_dat1()
{
	return inb(PIC1_DAT);
}

enum {statusSAVE,statusRESTORE};
char *irqerr="IRQ error";
char *irqerrbound="Anybody knows irq#%d";

#define LLOCK_ENTRY_INT_CLEARED 1
#define LLOCK_ENTRY_INT_SET 0

volatile struct irq_st	{
	unsigned char mask1;
	unsigned char mask2;
	int status;
	int proc_flag;
	int llock_entry; //Whether interrupts were enabled when llock was first called
}irq_st;

static void set_irq(unsigned char mask_register1,unsigned char mask_register2)
{                          
	write_dat0(mask_register1);
	write_dat1(mask_register2);
}

static void irq_remap(unsigned char irq_start,unsigned char irq_start2)
{
    write_cmd0(PIC_INIT);
    write_cmd1(PIC_INIT);
    write_dat0(irq_start);
    write_dat1(irq_start2);
    write_dat0(0x04);
    write_dat1(0x02);
    write_dat0(0x01);
    write_dat1(0x01);
    write_dat0(0x0);
    write_dat1(0x0);
}

int restore_irq_pm()
{
	irq_remap(IRQMAP1,IRQMAP2);
	set_irq(irq_st.mask1,irq_st.mask2);
	return 0;
}

int prepare_irq_rm()
{
	irq_remap(RMIRQ1,RMIRQ2);
	return 0;
}

int enable_irq(int irq_number)
{
	unsigned char pic;
	if(irq_number<8)	{
		pic=read_dat0();
		pic&=(0xff-pow(2,irq_number));
		irq_st.mask1=pic;
		write_dat0(pic);
	}
	else if(irq_number<16)	{
		pic=read_dat1();
		pic&=(0xff-pow(2,(irq_number-8)));
		irq_st.mask2=pic;
		write_dat1(pic);
	}
	else
		stop(irqerrbound,irq_number);
	return 0;
}

int disable_irq(int irq_number)
{
	
	unsigned char pic;
	if(irq_number<8)	{
		pic=read_dat0();
		pic|=(pow(2,irq_number));
		irq_st.mask1=pic;
		write_dat0(pic);
	}
	else if(irq_number<16)	{
		pic=read_dat1();
		pic|=(pow(2,(irq_number-8)));
		irq_st.mask2=pic;
		write_dat1(pic);
	}
	else
		stop(irqerrbound,irq_number);
	return 0;
}

int is_irq(int irq_number)
{
	unsigned char pic;
	if(irq_number<8)	{
		pic=read_dat0();
		return (pic&(pow(2,irq_number)));
	}
	else if(irq_number<16)	{
		pic=read_dat1();
		return (pic&(pow(2,(irq_number-8))));
	}
	else
		stop(irqerrbound,irq_number);
	return 0;
}

int irq_ended(int irq_number)
{
	if(irq_number<8)	{
		write_cmd0(0x20);
	}
	else if(irq_number<16)	{
		write_cmd1(0x20);
		write_cmd0(0x20);
	}
	else
		stop(irqerrbound,irq_number);
	return NULL;
}

static int irq_save()
{
	if(irq_st.mask1==statusRESTORE)	{
		irq_st.mask1=read_dat0();
		irq_st.mask2=read_dat1();
		irq_st.mask1=statusSAVE;
	}
	else
		stop(irqerr);
	return 0;
}

static int irq_restore()
{
	if(irq_st.mask1==statusSAVE)	{
		write_dat0(irq_st.mask1);
		write_dat1(irq_st.mask2);
		irq_st.mask1=statusRESTORE;
	}
	else
		stop(irqerr);
	return 0;
}

int irqs_enabled()
{
	int mask1,mask2;
	mask1=read_dat0();
	mask2=read_dat1();
	if(mask1==0xff && mask2==0xff)
		return 1;
	return NULL;
}

int irq_save_disable()
{
	irq_save();
	set_irq(MASKALL,MASKALL);
	return NULL;
}

int irq_restore_enable()
{
	return irq_restore();
}

/*llock() and llockfree() are very important kernel functions required for protection
of crucial code*/
int llock()
{
	if(irq_st.proc_flag<0)
		stop("Releasing unacquired kernel lock!");
	else if(irq_st.proc_flag>0 && _interrupt_flag())	{
		stop("Interrupts enabled in kernel lock!");
	}
	if(irq_st.proc_flag==0) {
		if(_interrupt_flag()) {	/*Interrupts are set, clear them*/
			irq_st.llock_entry=LLOCK_ENTRY_INT_SET;
			cli();
		} else	/*Leave flag cleared*/
			irq_st.llock_entry=LLOCK_ENTRY_INT_CLEARED;
	}
	irq_st.proc_flag++;	/*stack*/
	return SUCCESS;
}

int llockfree()
{
	irq_st.proc_flag--;
	if(irq_st.proc_flag<0)
		stop("Releasing unacquired kernel lock!");
	else if(irq_st.proc_flag>0 && _interrupt_flag())	{
		stop("Interrupts enabled in kernel lock!");
	}
	if(irq_st.proc_flag==0)	{	//Release the lock only if llock() and llockfree() were called the same number of times
		if(irq_st.llock_entry==LLOCK_ENTRY_INT_SET)	/*The normal case: Interrupts were enabled on the first call to llock()*/
			sti();
	}
	return SUCCESS;
}		

static int init_Done=0;
int init_8259()
{
	if(init_Done==8259)	{
		return -1;
	}
	irq_remap(IRQMAP1,IRQMAP2);
	irq_st.mask1=MASKALL;
	irq_st.mask2=MASKALL;
	irq_st.proc_flag=0;
	set_irq(irq_st.mask1,irq_st.mask2);
	_set_all_irqs();	//This asm function sets all hw ints to uirq#
	enable_irq(2);	//Enable the cascaded pic
	irq_st.status=statusRESTORE;
	irq_st.llock_entry=LLOCK_ENTRY_INT_SET;
	init_Done=8259;
	sti();	//Start interrupts for the first time...
	return 0;
}

static unsigned read_device(unsigned p0,unsigned p1,unsigned p2,unsigned p3)
{
	switch(p0)	{
		case READ_IS_IRQ: return is_irq(p1);
	}
	return 0; 
}

static unsigned write_device(unsigned p0,unsigned p1,unsigned p2,unsigned p3)
{
	switch(p0)	{
		case WRITE_PREPARE_IRQ_RM: return prepare_irq_rm();
		case WRITE_RESTORE_IRQ_PM: return restore_irq_pm();
		case WRITE_ENABLE_IRQ: return enable_irq(p1);
		case WRITE_DISABLE_IRQ: return disable_irq(p1);
		case WRITE_LLOCK: return llock();
		case WRITE_LLOCKFREE: return llockfree();
		case WRITE_IRQ_ENDED: return irq_ended(p1);
	}
	return 0;
}

kdrv i8259={
	.name="i8259",
	.init=init_8259,
	.reset=NULL,
	.deinit=NULL, 
	.read_device=read_device,
	.write_device=write_device,
	.device_irq=DEVICE_IRQ_UNUSED,
};
